/**************************************************************
 * Ben Stephenson       07025656
 * Mark Renney          07010390
 * Laurie Potts         07009314
 * Michael Peeke-Vout   07014312
 *
 * CM0034 Industrial Case Project
 * Conveyor Belt
 * main.c
 * 28/02/2011
 * Updated - 09/05/11 
 **************************************************************/

#include <stdbool.h>
#include <ucos_ii.h>
#include <board.h>
#include <bsp.h>
#include <osutils.h>
#include <leds.h>
#include <buttons.h>
#include <lcd.h>
#include <interface.h>
#include <conveyor.h>
#include <buffer.h>
#include <can.h>
#include <iolpc2378.h>
#include <messages.h>
#include <global_decs.h>
#include <conveyor_extras.h>




/*
*********************************************************************************************************
*                                            PRIORITIES
*********************************************************************************************************
*/

#define  APP_TASK_EMERGENCY_PRIO                  4
#define  APP_TASK_CAN_PRIO                        6
#define  APP_TASK_CONVEYOR_PRIO                   8
#define  APP_TASK_BUTTONS_PRIO                    14
#define  APP_TASK_DISPLAY_PRIO                    20

/*
*********************************************************************************************************
*                                            APPLICATION TASK STACKS
*********************************************************************************************************
*/

#define  APP_TASK_EMERGENCY_STK_SIZE            256
#define  APP_TASK_CAN_STK_SIZE                  256
#define  APP_TASK_CONVEYOR_STK_SIZE             256
#define  APP_TASK_BUTTONS_STK_SIZE              256
#define  APP_TASK_DISPLAY_STK_SIZE              256


static OS_STK appTaskEmergencyStk[APP_TASK_CAN_STK_SIZE];
static OS_STK appTaskCanHandlerStk[APP_TASK_CAN_STK_SIZE];
static OS_STK appTaskConveyorHandlerStk[APP_TASK_CONVEYOR_STK_SIZE];
static OS_STK appTaskButtonsStk[APP_TASK_BUTTONS_STK_SIZE];
static OS_STK appTaskDisplayStk[APP_TASK_CAN_STK_SIZE];

/*
*********************************************************************************************************
*                                            APPLICATION FUNCTION PROTOTYPES
*********************************************************************************************************
*/

static void appTaskEmergencyStop(void *pdata);
static void appTaskCanHandler(void *pdata);
static void appTaskConveyorHandler(void *pdata);
static void appTaskButtons(void *pdata);
static void appTaskDisplay(void *pdata);

/*
***************************************************************************
*                           OTHER LOCAL FUNCTION PROTOTYPES
***************************************************************************
*/

static void initEmergency(void);
static void startSystem(void);

/*
***************************************************************************
*                           GLOBAL VARIABLE DEFINITIONS
***************************************************************************
*/

messageData_t message;

/*
****************************************************************************
*                           GLOBAL FUNCTION DEFINITIONS
****************************************************************************
*/


int main() {

  /* Initialise the OS */
  OSInit();                                                   
  
  //start our system
  startSystem();  

  /* Start the OS */
  OSStart();     
  
  /* Should never arrive here */ 
  return 0;     
  
}//main

/*
***************************************************************************
*                    APPLICATION TASK DEFINITIONS
***************************************************************************
*/


/*************************************************
 * Function : appTaskDisplay
 * Desc     : 5hz screen refresh
 * Return   : Void
 * Params   : Void
 *
 *^z**********************************************/
static void appTaskDisplay(void *pdata) {
  
  
  //Do these outside loop, should decrease task time
  lcdSetTextPos(2, 1);
  lcdWrite("S1 : ");
  lcdSetTextPos(2, 2);
  lcdWrite("S2 : ");
  lcdSetTextPos(2, 3);
  lcdWrite("CSTATE : ");
  lcdSetTextPos(2, 4);
  lcdWrite("CMOVE : ");
  lcdSetTextPos(2, 5);
  lcdWrite("SYS : ");
  lcdSetTextPos(2, 6);
  lcdWrite("BOC   : ");
  lcdSetTextPos(2, 7);
  lcdWrite("READ   :  WRITE");
  
  uint8_t sys_state;
  messageData_t lastWrite; 
  bool conCanMove;
  
  while(true){
    
    //Sensor 1 status
    lcdSetTextPos(10, 1);
    if (conveyorItemPresent(CONVEYOR_SENSOR_1)) 
      lcdWrite("P"); 
    else
      lcdWrite("N");
    
    //Sensor 2 status   
    lcdSetTextPos(10, 2);
    if (conveyorItemPresent(CONVEYOR_SENSOR_2)) 
      lcdWrite("P");  
    else
      lcdWrite("N");
    
    //Conveyor belt status
    lcdSetTextPos(10, 3);
    if (conveyorGetState() == CONVEYOR_FORWARD)
      lcdWrite("FOR");
    else if (conveyorGetState() == CONVEYOR_REVERSE)
      lcdWrite("REV");
    else
      lcdWrite("OFF");  
    
    //Conveyor has permission to move
    lcdSetTextPos(10, 4);
    conCanMove = getConMovePerm(); 
    
    if(conCanMove == true)
      lcdWrite("Y");
    else
      lcdWrite("N");
    
    
    //Current System State
    lcdSetTextPos(10, 5);
    sys_state = getState();
    
    if(sys_state == RUNNING)
      lcdWrite("RUN     ");  
    else if(sys_state == PAUSE)
      lcdWrite("PAUSE   ");   
    else if(sys_state == STOP)
      lcdWrite("STOP    ");  
    else if(sys_state == STOPPING)
      lcdWrite("STOPPING ");      
    
    
    //last read/write from the can
    getLastMsg(&lastWrite, MY_ID);
  
    lcdSetTextPos(2, 8);
    lcdWrite("T.ID : %d - %d", message.targetID, lastWrite.targetID);
    lcdSetTextPos(2, 9);
    lcdWrite("O.ID : %d - %d", message.originatorID, lastWrite.originatorID);
    lcdSetTextPos(2, 10);
    lcdWrite("MES : %d - %d", message.content, lastWrite.content); 
    
    OSTimeDly(200);
    
  }//while
  
}//appTaskDisplay


/*************************************************
 * Function : appTaskButtons
 * Desc     : Polls the buttons for simulations
 * Return   : Void
 * Params   : Void
 *
 *^z**********************************************/
static void appTaskButtons(void *pdata) {
  
  while(true){
    
    //Simumate Emergency
    if (isButtonPressed(JS_LEFT))
      initEmergency();   
    
    //Simulate pause
    if (isButtonPressed(JS_UP))
      setState(PAUSE);
    
    //Simulate stop
    else if (isButtonPressed(JS_RIGHT))
      setState(STOPPING);  
    
    //Simulate resume
    else if (isButtonPressed(JS_DOWN))
      setState(RUNNING);   
    
    //simulate conveyor movement permissions
    else if(isButtonPressed(BUT_1))
      setConMovePerm(true);
    else if(isButtonPressed(BUT_2))
      setConMovePerm(false);
    
    OSTimeDly(20);
    
  }//while
  
}//appTaskButtons




/*************************************************
 * Function : appTaskConveyorHandler
 * Desc     : Handles conveyor belt actions.
 * Return   : Void
 * Params   : Void
 *
 *^z**********************************************/
void appTaskConveyorHandler(void *pdata){
    
  uint32_t startTime = 0;
  uint32_t now = 0;
  uint32_t timeout = 8000; //8 seconds
  
  while(true){
    
      
      //if item is present in sensor 1
      if (conveyorItemPresent(CONVEYOR_SENSOR_1)) {
        
        //If sensor 1 is triggered for the first time stop belt
        if(conveyorGetState() != CONVEYOR_OFF){   
          setConMovePerm(false);
          conveyorSetState(CONVEYOR_OFF);
        }
        
        //Notify arm2 until they respond
        messageData_t tempMess;
        getLastMsg(&tempMess, ID_ARM2);
        
        if(tempMess.content != MSG_ACKNOLEDGE){
          messageData_t tempWrite = {ID_ARM2, MY_ID, MSG_SENS1_FULL};
          writeMsgToCan(&tempWrite); 
        }
      }
      
      //Only move conveyor is safe, check to see if it needs to
      else {
        
        //If block is in sensor two range and it's safe to move  
        if (conveyorItemPresent(CONVEYOR_SENSOR_2) && getConMovePerm()  
            && !(getState() == PAUSE) && !(getState() == STOP)) {
          
          //conveyor isn't moving yet, start it
          if(conveyorGetState() == CONVEYOR_OFF){
            
            conveyorSetState(CONVEYOR_REVERSE);           
            startTime = OSTimeGet(); 
          }
        }
        
        //Conveyor is moving, test for timeout
        else if(conveyorGetState() != CONVEYOR_OFF){  
          
          //Timeout has occured, it's gone south
          now = OSTimeGet();
          if((now-startTime)>timeout){
            //Tell control panel there is an emergency
            messageData_t tempWrite = {ID_CONTROLP, MY_ID, MSG_EMERG_STOP};
            writeMsgToCan(&tempWrite); 
          }
        }   
      }
      
      //Handle stop
      if(getState() == STOPPING){
        
        //nothing left to process
        if(conveyorGetState() == CONVEYOR_OFF && 
           !conveyorItemPresent(CONVEYOR_SENSOR_1)
             && !conveyorItemPresent(CONVEYOR_SENSOR_2)){
         
          setState(STOP);    
          //Tell control panel this component has stopped
          messageData_t tempWrite = {ID_CONTROLP, MY_ID, MSG_STOP};
          writeMsgToCan(&tempWrite); 
        }
      }
      
    OSTimeDly(100);
    
  }//while
   
}//appTaskConveyorHandler

/*************************************************
 * Function : appTaskCanHandler
 * Desc     : Polls the can for incoming messages.
 * Return   : Void
 * Params   : Void
 *
 * TODO     : Make this interrupt driven
 *^z**********************************************/
void appTaskCanHandler(void *pdata){
  
  
  //Initilise the hardware
  bspInit();
  interfaceInit(CONVEYOR);
  osStartTick();
  
  //notify control panel of startup
  messageData_t readyMess = {ID_CONTROLP, MY_ID, MSG_READY};
  writeMsgToCan(&readyMess);
  
  while(true){
    
    //grab message from CAN
    if(readMsgFromCan(&message)){     
      
      //Emergency Stop
      if(message.content == MSG_EMERG_STOP)    
        initEmergency();
      
      //Stop - components will stop when idle.
      else if(message.content == MSG_STOP)
        setState(STOPPING);
      
      //Pause
      else if(message.content == MSG_PAUSE){
        
        setState(PAUSE);
        OSTimeDly(10 * MY_ID);
        messageData_t tempWrite = {ID_CONTROLP, MY_ID, MSG_PAUSE};
        writeMsgToCan(&tempWrite); 
      }
      
      //Resume
      else if(message.content == MSG_RESUME){
        
        setState(RUNNING);
        OSTimeDly(10 * MY_ID);
        messageData_t tempWrite = {ID_CONTROLP, MY_ID, MSG_RESUME};
        writeMsgToCan(&tempWrite); 
      }
      
      //Deals with completion messages
      else if(message.content == MSG_PICKUP_DEPOSIT_DONE){
       
        uint8_t status;
       
        //if arm2 it's a pickup succeeded request
        if(message.originatorID == ID_ARM2){
         
          if(conveyorItemPresent(CONVEYOR_SENSOR_1))
            status = MSG_SENS1_FULL;   
          else
            status = MSG_SENS1_EMPTY;
        }
        
        //if arm1 it's a deposit succeeded request
        else if(message.originatorID == ID_ARM1){
         
          if(conveyorItemPresent(CONVEYOR_SENSOR_2)){
            status = MSG_SENS2_FULL;
            setConMovePerm(true);
          }
          else
            status = MSG_SENS2_EMPTY;  
        }
       
        //transmits appropriate response
        messageData_t tempMess = {message.originatorID, MY_ID, status};
        writeMsgToCan(&tempMess); 
      }
      
      //Sensor one status request
      else if(message.content == MSG_CON_SENS1_STAT_REQ){
       
        //If sensor1 is full respond with that
        uint8_t status = MSG_SENS1_EMPTY;
       
        if(conveyorItemPresent(CONVEYOR_SENSOR_1))
          status = MSG_SENS1_FULL;
       
        messageData_t tempMess = {message.originatorID, MY_ID, status};
        writeMsgToCan(&tempMess); 
      }
      
      //Sensor two status request
      else if(message.content == MSG_CON_SENS2_STAT_REQ){
          
            
        //If sensor is full of conveyor is moving, it's not safe
        //to deposit.
        uint8_t status = MSG_SENS2_EMPTY;
          
        if(conveyorItemPresent(CONVEYOR_SENSOR_2) || 
                    (conveyorGetState() != CONVEYOR_OFF))
           status = MSG_SENS2_FULL;       

        messageData_t tempMess = {message.originatorID, MY_ID, status};
        writeMsgToCan(&tempMess);    
      }

          
    }
 
    OSTimeDly(10);
  }//while

}//appTaskCanHandler


/*************************************************
 * Function : appTaskEmergencyStop
 * Desc     : Kill system and notify user vua GUI.
 * Return   : Void
 * Params   : Void
 *
 * TODO     : Reset on button press?
 *^z**********************************************/
void appTaskEmergencyStop(void *pdata){
  
  
  //Kill this to ensure can messages have no further control of system state
  OSTaskDel(APP_TASK_CAN_PRIO);
  OSTaskDel(APP_TASK_CONVEYOR_PRIO); 
  OSTaskDel(APP_TASK_DISPLAY_PRIO);
  OSTaskDel(APP_TASK_BUTTONS_PRIO);

  //Prevents multiple nodes flooding the CAN bus after a global broadcast
  OSTimeDly(10 * MY_ID);
  
  //Tell control panel this component is in emergency state
  messageData_t tempWrite = {ID_CONTROLP, MY_ID, MSG_EMERG_STOP};
  writeMsgToCan(&tempWrite); 
  

  //notify user of emergency situation visually
  printf( "\f" );
  lcdSetTextPos(2, 6);
  lcdWrite("SHUTDOWN");
  lcdSetTextPos(2, 10);
  lcdWrite("HOLD BUT2 FOR RESET");
  
  //** LAST READ/WRITE FROM THE CAN **//
  messageData_t lastWrite; 
  getLastMsg(&lastWrite, MY_ID);
  
  lcdSetTextPos(2, 2);
  lcdWrite("T.ID : %d - %d", message.targetID, lastWrite.targetID);
  lcdSetTextPos(2, 3);
  lcdWrite("O.ID : %d - %d", message.originatorID, lastWrite.originatorID);
  lcdSetTextPos(2, 4);
  lcdWrite("MES : %d - %d", message.content, lastWrite.content); 
  
  ledSetState(USB_LINK_LED, LED_OFF);
  ledSetState(USB_CONNECT_LED, LED_ON);
  
  //flag for screen flash
  bool flash = false;
  
  while(true){
    
    //If users hits reset, go to default, start system,
    //destroy self and then break for good measure.
    if (isButtonPressed(BUT_2)) {
      
      printf( "\f" );
      ledSetState(USB_LINK_LED, LED_OFF);
      ledSetState(USB_CONNECT_LED, LED_OFF);
      
      //start system then suicide
      startSystem();
      OSTaskDel(OS_PRIO_SELF);
      
      //shouldn't reach here
      break;
    }
    
    //flash screen
    lcdSetTextPos(2, 7);
    if(flash){
      lcdWrite("                    ");
      flash = false;
    }
    else{
      
      lcdWrite("EMERGENCY STOP!");
      flash = true;
    }
    
    //flash LEDs
    ledToggle(USB_LINK_LED);
    ledToggle(USB_CONNECT_LED);
    
       
    OSTimeDly(1000);
    
  }//while
  
}//appTaskEmergencyStop


/*
*************************************************************************
                       LOCAL FUNCTION DEFINITIONS
*************************************************************************
*/


/*************************************************
 * Function : initEmergency
 * Desc     : Initiate task to handle emergency stop + kill conveyor.
 * Return   : Void
 * Params   : Void
 *^z**********************************************/
void initEmergency(void){
  
  conveyorSetState(CONVEYOR_OFF);
  OSTaskCreate(appTaskEmergencyStop,                               
               (void *)0,
               (OS_STK *)&appTaskEmergencyStk[APP_TASK_EMERGENCY_STK_SIZE - 1],
               APP_TASK_EMERGENCY_PRIO);
}



/*************************************************
 * Function : startSystem
 * Desc     : Start the system into ready state.
 * Return   : Void
 * Params   : Void
 *
 *^z**********************************************/
void startSystem(void){
  
  //reset system vars
  setConMovePerm(false);
  setState(RUNNING);
  
    /* Create Tasks */
  OSTaskCreate(appTaskButtons,                               
           (void *)0,
           (OS_STK *)&appTaskButtonsStk[APP_TASK_BUTTONS_STK_SIZE - 1],
           APP_TASK_BUTTONS_PRIO);

  OSTaskCreate(appTaskConveyorHandler,                               
           (void *)0,
           (OS_STK *)&appTaskConveyorHandlerStk[APP_TASK_CONVEYOR_STK_SIZE - 1],
           APP_TASK_CONVEYOR_PRIO);
  
   OSTaskCreate(appTaskCanHandler,                               
           (void *)0,
           (OS_STK *)&appTaskCanHandlerStk[APP_TASK_CAN_STK_SIZE - 1],
           APP_TASK_CAN_PRIO);
  
  OSTaskCreate(appTaskDisplay,                               
           (void *)0,
           (OS_STK *)&appTaskDisplayStk[APP_TASK_DISPLAY_STK_SIZE - 1],
           APP_TASK_DISPLAY_PRIO);

  

}//startSystem